#include <rclcpp/rclcpp.hpp>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cstring>
#include <fcntl.h>  // 用于设置套接字为非阻塞模式
#include "hand_msgs/msg/set_param.hpp"  // 包含自定义消息类型

// 将 float 类型数据转换成 unsigned char 数组
void float2uchar(float value, unsigned char* buffer) {
    uint32_t float_data = *reinterpret_cast<uint32_t*>(&value);
    buffer[0] = (float_data >> 24) & 0xFF;
    buffer[1] = (float_data >> 16) & 0xFF;
    buffer[2] = (float_data >> 8) & 0xFF;
    buffer[3] = float_data & 0xFF;
}

// 将 unsigned char 数组转换成 float 类型数
float uchar2float(const unsigned char* buffer) {
    uint32_t float_data = (buffer[0] << 24) | (buffer[1] << 16) | (buffer[2] << 8) | buffer[3];
    return *reinterpret_cast<float*>(&float_data);
}

// 测试 float2uchar 和 uchar2float 函数
void test() {
    float test_value = 2;
    unsigned char buffer[12];
    buffer[0] = 0xff;
    buffer[1] = 0xa5;
    buffer[2] = 1;
    buffer[3] = 1;
    float2uchar(test_value, &buffer[4]);

    // 计算前 8 字节之和
    uint32_t sum = 0;
    for (int i = 0; i < 8; ++i) {
        sum += buffer[i];
    }

    // 将和存储到最后 4 个字节
    buffer[8] = (sum >> 24) & 0xFF;
    buffer[9] = (sum >> 16) & 0xFF;
    buffer[10] = (sum >> 8) & 0xFF;
    buffer[11] = sum & 0xFF;

    // 打印 12 字节数组到一行
    std::stringstream ss;
    ss << "Generated 12-byte buffer: ";
    for (int i = 0; i < 12; ++i) {
        ss << "0x" << std::hex << static_cast<int>(buffer[i]);
        if (i < 11) {
            ss << " ";
        }
    }
    RCLCPP_INFO(rclcpp::get_logger("test"), "%s", ss.str().c_str());

    // 测试 uchar2float 转换回来的值
    float result = uchar2float(&buffer[4]);
    RCLCPP_INFO(rclcpp::get_logger("test"), "Test value: %f, Result: %f", test_value, result);
}

class SetParamNode : public rclcpp::Node
{
public:
    SetParamNode() : Node("ros2_gui_param_node"), is_shutting_down_(false)
    {
        // 创建 UDP 套接字
        sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd_ < 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to create socket");
            return;
        }

        // 设置服务器地址
        memset(&server_addr_, 0, sizeof(server_addr_));
        server_addr_.sin_family = AF_INET;
        server_addr_.sin_addr.s_addr = INADDR_ANY;
        server_addr_.sin_port = htons(12345);

        // 绑定套接字
        if (bind(sockfd_, (struct sockaddr *)&server_addr_, sizeof(server_addr_)) < 0) {
            RCLCPP_ERROR(this->get_logger(), "Failed to bind socket");
            close(sockfd_);
            return;
        }

        RCLCPP_INFO(this->get_logger(), "UDP server started on port 12345");

        // 运行测试函数
        test();

        // 创建定时器以接收数据，将周期改为 20 毫秒
        timer_ = this->create_wall_timer(std::chrono::milliseconds(20), std::bind(&SetParamNode::receive_data, this));

        // 创建发布者
        publisher_ = this->create_publisher<hand_msgs::msg::SetParam>("motor_param", 10);
    }

    ~SetParamNode()
    {
        is_shutting_down_ = true;
        // 停止定时器
        if (timer_) {
            timer_->cancel();
        }
        // 确保定时器回调不再执行
        rclcpp::sleep_for(std::chrono::milliseconds(200));

        if (sockfd_ >= 0) {
            // 设置套接字为非阻塞模式
            int flags = fcntl(sockfd_, F_GETFL, 0);
            fcntl(sockfd_, F_SETFL, flags | O_NONBLOCK);

            // 优雅地关闭套接字
            shutdown(sockfd_, SHUT_RDWR);
            close(sockfd_);
        }
    }

private:
    void receive_data()
    {
        if (is_shutting_down_) {
            return;
        }

        unsigned char buffer[12];
        socklen_t client_addr_len = sizeof(client_addr_);
        ssize_t recv_len = recvfrom(sockfd_, buffer, sizeof(buffer), 0, (struct sockaddr *)&client_addr_, &client_addr_len);
        if (recv_len < 0) {
            if (!is_shutting_down_) {
                RCLCPP_ERROR(this->get_logger(), "Failed to receive data");
            }
            return;
        }

        if (recv_len == 12) {
            // 校验第一个字节和第二个字节
            if (buffer[0] != 0xff || buffer[1] != 0xa5) {
                RCLCPP_ERROR(this->get_logger(), "First or second byte validation failed");
                return;
            }

            // 校验数据
            uint32_t sum = 0;
            for (int i = 0; i < 8; ++i) {
                sum += buffer[i];
            }
            uint32_t checksum = (buffer[8] << 24) | (buffer[9] << 16) | (buffer[10] << 8) | buffer[11];

            if (sum == checksum) {
                RCLCPP_INFO(this->get_logger(), "Data validation passed");
                func_ = static_cast<int>(buffer[2]);
                id_ = static_cast<int>(buffer[3]);
                // 5 至 8 字节合成转换成 float
                data_ = uchar2float(&buffer[4]);

                RCLCPP_INFO(this->get_logger(), "func_: %d, id_: %d, data_: %f", func_, id_, data_);

                // 发布消息
                auto msg = std::make_shared<hand_msgs::msg::SetParam>();
                msg->func = func_;
                msg->id = id_;
                msg->data = data_;
                publisher_->publish(*msg);
            } else {
                RCLCPP_ERROR(this->get_logger(), "Data validation failed");
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "Received data length is not 12 bytes");
        }
    }

    int sockfd_;
    struct sockaddr_in server_addr_;
    struct sockaddr_in client_addr_;
    rclcpp::TimerBase::SharedPtr timer_;
    int func_;
    int id_;
    float data_;
    bool is_shutting_down_;
    rclcpp::Publisher<hand_msgs::msg::SetParam>::SharedPtr publisher_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SetParamNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}    